#include "moving_cmd.h"
#include "axisgroup/pfsm_free.h"
#include "axisgroup/simple_tp.h"
#include "axisgroup/pfsm_coord.h"
#include "tp/tpItf.hh"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_set.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "zuc/task/motion_al.h"
#include "kine/kine.h"
#include "dyna/jaka_dyn_intf.h"
#include "log/zuclog.h"
#include "errcode.h"
#include "safety/io/safeio.h"

#include "zuc/nml_intf/motion_types.h"
#include "motion/mot_priv.h"

#include <math.h>

#define MOTCMD_INFO(fmt, ...) zuclog_info("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("MOTCMD", fmt, ##__VA_ARGS__)

int CmdInFreeMode::active_callback()
{
    if (zucmotStatus->net_feed_scale < 0.0001)
    {
        /* don't jog if feedhold is on or if feed override is zero */
        return 0;
    }
    if (zucmotStatus->three_position_enable_limit == 1)  // #TODO
    {
        //三位置开关处于打开状态不能运动
        reportError(ERROR_THREE_POSITION_ENABLING_LIMIT, "Can't jog when the three positions enable limits");
        return -1;
    }
    if (safety::SafeIOMgr::instance().is_protective_stop())
    {
        MOTCMD_ERR("Cannot do JOG when in protective_stop status");
        return -1;
    }
    if (this->jog_axis_ < 0)
    {
        return -1;
    }
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();

    switch (jog_mode_)
    {
    case JOG_TYPE::JOG_WORLD:
    case JOG_TYPE::JOG_TOOL:
        if (jog_axis_ >= 6)
        {
            MOTCMD_ERR("Robot %d don't allow jog cart axis %d", jog_axis_);
            return -1;
        }
        break;
    case JOG_TYPE::JOG_JOINT:
        if (jog_axis_ >= status.robot_joint_num)
        {
            MOTCMD_ERR("Robot %d don't allow jog joint %d", jog_axis_);
            return -1;
        }
        break;
    default:
        return -1;
    }

    return 0;
}

int CmdInFreeMode::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    if (robot->get_cur_plan_mode() != axisgroup::PFsmState::PlanMode::PLAN_FREE)
    {
        if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
        {
            MOTCMD_ERR("Robot %d is not ready for a jog command : moving state: %s, plan mode : %s",
                       group_id_,
                       axisgroup::AxisGroup::run_state_string(robot->get_state()),
                       axisgroup::Robot::get_plan_mode_stirng(robot->get_cur_plan_mode()));
            return -1;
        }
        // 自动切至free模式
        robot->switch_plan_mode(axisgroup::PFsmState::PlanMode::PLAN_FREE);
    }
    return 0;
}

static int joint_velocity_limit_is_zero(int num)
{
    if (num >= 0 && num < zucmotConfig->numJoints)
    {
        if (fabs(joints[num].vel_limit) < 0.001)
        {
            reportError(ERROR_JOINT_VELOCITY_LIMIT_IS_ZERO, "joint[%d] velocity limit is zero ", num);
            return 1;
        }
        else
        {
            return 0;
        }
    }
    else if (num == -1)  // Jog Axis
    {
        for (int index = 0; index < zucmotConfig->numJoints; index++)
        {
            if (fabs(joints[index].vel_limit) < 0.001)
            {
                reportError(ERROR_JOINT_VELOCITY_LIMIT_IS_ZERO, "joint[%d] velocity limit is zero  1 ", index);
                return 1;
            }
        }
    }
    return 0;
}

static int joint_jog_ok(int joint_num, double vel)
{
    zucmot_joint_t* joint;
    int neg_limit_override, pos_limit_override;

    /* point to joint data */
    joint = &joints[joint_num];
    /* are any limits for this joint overridden? */
    neg_limit_override = zucmotStatus->overrideLimitMask & (1 << (joint_num * 2));
    pos_limit_override = zucmotStatus->overrideLimitMask & (2 << (joint_num * 2));
    if (neg_limit_override && pos_limit_override)
    {
        /* both limits have been overridden at the same time.  This
		 happens only when they both share an input, but means it
		 is impossible to know which direction is safe to move.  So
		 we skip the following tests... */
        return 1;
    }
    if (joint_num < 0 || joint_num >= zucmotConfig->numJoints)
    {
        reportError(INVALID_JOINT_NUM, ("Can't jog invalid joint number %d."), ONE_BASED(joint_num));
        return 0;
    }
    if (vel > 0.0 && GET_JOINT_PHL_FLAG(joint))
    {
        reportError(JOINT_HIT_POS_HLIMIT, ("Can't jog joint %d further past max hard limit."), ONE_BASED(joint_num));
        return 0;
    }
    if (vel < 0.0 && GET_JOINT_NHL_FLAG(joint))
    {
        reportError(JOINT_HIT_NEG_HLIMIT, ("Can't jog joint %d further past min hard limit."), ONE_BASED(joint_num));
        return 0;
    }

    if (vel > 0.0 && ((joint->pos_cmd > joint->max_pos_limit - 0.0001)))
    {
        reportError(JOG_TARGET_EXCEED_POS_SLIMIT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
                    ("Can't jog joint %d further past max soft limit."),
                    ONE_BASED(joint_num));
        return 0;
    }
    if (vel < 0.0 && ((joint->pos_cmd < joint->min_pos_limit + 0.0001)))
    {
        reportError(JOG_TARGET_EXCEED_NEG_SLIMIT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
                    ("Can't jog joint %d further past min soft limit."),
                    ONE_BASED(joint_num));
        return 0;
    }
    /* okay to jog */
    return 1;
}

int CmdJogStop::process()
{
    MOTCMD_DEBUG("JOG_STOP");
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto free = std::static_pointer_cast<axisgroup::PFsmFree>(robot->get_free_pfsm());
    free->abort_plan();
    return 0;
}

int CmdJogCont::active_callback()
{
    int err = CmdInFreeMode::active_callback();
    if (err)
    {
        return err;
    }
    joint_velocity_limit_is_zero(jog_axis_);
    return 0;
}

// 以关节限位为终点,因此会一直运动，直至达到限位或外部调用了JOGSTOP
int CmdJogCont::process()
{
    if (CmdInFreeMode::process())
    {
        return -1;
    }
    MOTCMD_DEBUG("JOG_CONT  mode %d, axis %d\n", jog_mode_, jog_axis_);
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto free = std::static_pointer_cast<axisgroup::PFsmFree>(robot->get_free_pfsm());
    auto config = robot->get_robot_active_config();
    if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
    {
        // 机器人已经在动了
        free->reset_jog_countdown();
        return 0;
    }
    free->is_in_incr = false;
    if (jog_mode_ == JOG_TYPE::JOG_JOINT)
    {
        free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_JOINT);
        /* don't jog further onto limits */
        if (!joint_jog_ok(jog_axis_, vel_))
        {
            // SET_JOINT_ERROR_FLAG(joint, 1);
            return -1;
        }
        auto free_tp = &free->free_tp_[jog_axis_];

        /* set destination of jog */
        free_tp->pos_cmd = vel_ > 0 ? config.joint_limit.max[jog_axis_] : config.joint_limit.min[jog_axis_];
        free_tp->max_vel = fabs(vel_);  // FREE 中会对此速度再根据配置做限制
        free_tp->max_acc = config.jog.j_acc[jog_axis_];
        free_tp->enable = 1;
    }
    else
    {
        if (jog_mode_ == JOG_TYPE::JOG_WORLD)
        {
            free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_CART_WORLD);
        }
        else
        {
            free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_CART_TOOL);
        }
        auto free_tp_cart = &free->free_tp_cart_[jog_axis_];
        free_tp_cart->pos_cmd = vel_ > 0.0 ? 1000000 : -1000000;  // 无穷远
        free_tp_cart->max_vel = fabs(vel_);
        free_tp_cart->max_acc = jog_axis_ < 3 ? config.jog.acc.trans : config.jog.acc.ori;
        free_tp_cart->enable = 1;
    }
    free->reset_jog_countdown();
    return 0;
}

int CmdJogIncr::active_callback()
{
    int err = CmdInFreeMode::active_callback();
    if (err)
    {
        return err;
    }
    return 0;
}

static double adapt_step_acc(double step_offset, std::vector<double> step_acc, double max_acc)
{
    if (step_acc.size() < 6)
    {
        MOTCMD_ERR("Step acc seems not be load appropriately!");
        return max_acc;
    }
    double acc = 0;
    if (step_offset < 0.05)
    {
        acc = step_acc[6];
    }
    else if (step_offset < 0.1)
    {
        acc = step_acc[5];
    }
    else if (step_offset < 0.5)
    {
        acc = step_acc[4];
    }
    else if (step_offset < 1.0)
    {
        acc = step_acc[3];
    }
    else if (step_offset < 5)
    {
        acc = step_acc[2];
    }
    else if (step_offset < 10)
    {
        acc = step_acc[1];
    }
    else
    {
        acc = step_acc[0];
    }
    if (acc > max_acc)
    {
        return max_acc;
    }
    else if (acc < 1.0)
    {
        return 1.0;
    }
    return acc;
}

int CmdJogIncr::process()
{
    if (CmdInFreeMode::process())
    {
        return -1;
    }
    /* do an incremental jog */
    MOTCMD_DEBUG("JOG_INCR mode %d, axis %d", jog_mode_, jog_axis_);
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto free = std::static_pointer_cast<axisgroup::PFsmFree>(robot->get_free_pfsm());
    auto config = robot->get_robot_active_config();
    if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
    {
        // 机器人已经在FREE模式了，且还在动
        free->reset_jog_countdown();
        return 0;
    }
    if (free->is_in_incr)
    {
        // 重复的INCR指令，不响应
        return -1;
    }
    free->is_in_incr = true;

    joint_velocity_limit_is_zero(jog_axis_);

    if (jog_mode_ == JOG_TYPE::JOG_JOINT)
    {
        free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_JOINT);
        /* don't jog further onto limits */
        if (!joint_jog_ok(jog_axis_, vel_))
        {
            // SET_JOINT_ERROR_FLAG(joint, 1);
            return -1;
        }
        auto free_tp = &free->free_tp_[jog_axis_];

        /* set target position for jog */
        double tmp1 = vel_ > 0.0 ? free_tp->pos_cmd + offset_ : free_tp->pos_cmd - offset_;
        if ((tmp1 > config.joint_limit.max[jog_axis_]) && (vel_ > 0.0))
        {
            tmp1 = config.joint_limit.max[jog_axis_];
            reportError(JOG_TARGET_EXCEED_POS_SLIMIT_J1 + (jog_axis_ << ERROR_OFFSET_FOR_JOINT_NUM),
                        ("Can't jog joint %d further past max soft limit."),
                        ONE_BASED(jog_axis_));
            return -1;
        }
        if ((tmp1 < config.joint_limit.min[jog_axis_]) && (vel_ < 0.0))
        {
            tmp1 = config.joint_limit.min[jog_axis_];
            reportError(JOG_TARGET_EXCEED_NEG_SLIMIT_J1 + (jog_axis_ << ERROR_OFFSET_FOR_JOINT_NUM),
                        ("Can't jog joint %d further past min soft limit."),
                        ONE_BASED(jog_axis_));
            return -1;
        }
        free_tp->pos_cmd = tmp1;
        free_tp->max_vel = fabs(vel_);
        free_tp->max_acc = config.jog.j_acc[jog_axis_];
        free_tp->enable = 1;
    }
    else
    {
        double target = 0;
        auto free_tp_cart = &free->free_tp_cart_[jog_axis_];
        if (jog_mode_ == JOG_TYPE::JOG_WORLD)
        {
            free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_CART_WORLD);
            target = vel_ > 0.0 ? free_tp_cart->pos_cmd + offset_ : free_tp_cart->pos_cmd - offset_;
        }
        else
        {
            free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_CART_TOOL);
            target = vel_ > 0 ? offset_ : -offset_;
        }
        free_tp_cart->pos_cmd = target;
        free_tp_cart->max_vel = fabs(vel_);
        if (jog_axis_ < 3)
        {
            free_tp_cart->max_acc = adapt_step_acc(offset_, config.jog.pos_jog_acc, config.jog.acc.trans);
        }
        else
        {
            free_tp_cart->max_acc = adapt_step_acc(offset_, config.jog.ori_jog_acc, config.jog.acc.ori);
        }
        free_tp_cart->enable = 1;
        MOTCMD_DEBUG("JOG_INCR Max Acc is %lf", free_tp_cart->max_acc);
    }
    free->reset_jog_countdown();
    return 0;
}

int CmdJogAbs::active_callback()
{
    int err = CmdInFreeMode::active_callback();
    if (err)
    {
        return err;
    }
    if (jog_mode_ == JOG_TYPE::JOG_TOOL)
    {
        MOTCMD_ERR("JOG ABS is not supported for tool jogging");
        return -1;
    }
    joint_velocity_limit_is_zero(jog_axis_);
    return 0;
}

int CmdJogAbs::process()
{
    if (CmdInFreeMode::process())
    {
        return -1;
    }
    /* do an absolute jog */
    MOTCMD_DEBUG("JOG_ABS, mode %d, axis %d", jog_mode_, jog_axis_);
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto free = std::static_pointer_cast<axisgroup::PFsmFree>(robot->get_free_pfsm());
    auto config = robot->get_robot_active_config();
    if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
    {
        // 机器人已经在FREE模式了，且还在动
        free->reset_jog_countdown();
        return -1;
    }
    free->is_in_incr = false;
    if (jog_mode_ == JOG_TYPE::JOG_JOINT)
    {
        free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_JOINT);
        auto free_tp = &free->free_tp_[jog_axis_];
        /* don't jog further onto limits */
        if (!joint_jog_ok(jog_axis_, vel_))
        {
            return -1;
        }
        /* don't jog past limits */
        free_tp->pos_cmd = offset_;
        if (free_tp->pos_cmd > config.joint_limit.max[jog_axis_])
        {
            free_tp->pos_cmd = config.joint_limit.max[jog_axis_];
        }
        if (free_tp->pos_cmd < config.joint_limit.min[jog_axis_])
        {
            free_tp->pos_cmd = config.joint_limit.min[jog_axis_];
        }
        free_tp->max_vel = fabs(vel_);
        free_tp->max_acc = config.jog.j_acc[jog_axis_];
        free_tp->enable = 1;
    }
    else
    {
        free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_CART_WORLD);
        // 仅允许世界坐标系运动, 这里的机制沿用以前，故实际上姿态的绝对值并非是绝对值，而是增量
        auto free_tp_cart = &free->free_tp_cart_[jog_axis_];
        free_tp_cart->pos_cmd = offset_;
        free_tp_cart->max_vel = fabs(vel_);
        free_tp_cart->max_acc = jog_axis_ < 3 ? config.jog.acc.trans : config.jog.acc.ori;
        free_tp_cart->enable = 1;
    }
    free->reset_jog_countdown();
    return 0;
}

int CmdJogMultiJoint::active_callback()
{
    int err = CmdInFreeMode::active_callback();
    if (err)
    {
        return err;
    }
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
    for (int i = 0; i < status.robot_joint_num; i++)
    {
        if (target_[i] > status.robot_config.joint_limit.max[i] || target_[i] < status.robot_config.joint_limit.min[i])
        {
            MOTCMD_ERR("JOG_MULTI, out of reach: axis_num %d target %lf", i, target_[i]);
            return -1;
        }
    }
    return 0;
}

int CmdJogMultiJoint::process()
{
    if (CmdInFreeMode::process())
    {
        return -1;
    }
    MOTCMD_DEBUG("JOG_MULTI, target position: %s", target_.val_str().c_str());
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto free = std::static_pointer_cast<axisgroup::PFsmFree>(robot->get_free_pfsm());
    auto config = robot->get_robot_active_config();
    if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
    {
        // 机器人已经在FREE模式了，且还在动
        return -1;
    }
    if (free->switch_jog_type(axisgroup::PFsmFree::JogType::JOG_JOINT))
    {
        return -1;
    }
    free->is_in_incr = false;

    RobJointVal d_pos;
    RobJointVal cur_pos = robot->get_cur_cmd_pos().jpos;
    RobJointVal max_vel = config.jog.j_vel;
    for (size_t i = 0; i < max_vel.MAX_AXIS; i++)
    {
        if (max_vel[i] > fabs(vel_))
        {
            max_vel[i] = fabs(vel_);
        }
    }
    RobJointVal max_acc = config.jog.j_acc;
    int main_axis = -1;  // 运动时间最长的轴
    double max_t = -1;
    for (size_t i = 0; i < robot->joint_num(); i++)
    {
        d_pos[i] = fabs(target_[i] - cur_pos[i]);
        double t = 0;
        if (d_pos[i] < 1e-5)
        {
            t = 0;
        }
        else if (d_pos[i] > max_vel[i] * max_vel[i] / max_acc[i])
        {
            t = d_pos[i] / max_vel[i] + max_vel[i] / max_acc[i];
        }
        else
        {
            t = sqrt(2 * d_pos[i] / max_acc[i]);
        }
        if (max_t < t)
        {
            max_t = t;
            main_axis = (int)i;
            continue;
        }
    }
    if (main_axis == -1)
    {
        return 0;
    }
    RobJointVal v_real;
    for (size_t i = 0; i < robot->joint_num(); i++)
    {
        if (i == (size_t)main_axis)
        {
            v_real[i] = max_vel[i];
        }
        else
        {
            v_real[i] = (max_acc[i] * max_t - sqrt(max_acc[i] * max_acc[i] * max_t * max_t - 4 * max_acc[i] * d_pos[i])) / 2.0;
        }
    }
    MOTCMD_DEBUG("JOG_MULTI, main axis %d: running time %lf, vel %s", main_axis, max_t, v_real.val_str().c_str());
    for (size_t i = 0; i < robot->joint_num(); i++)
    {
        free->free_tp_[i].pos_cmd = target_[i];
        free->free_tp_[i].max_vel = v_real[i];
        free->free_tp_[i].max_acc = max_acc[i];
        free->free_tp_[i].enable = 1;
    }
    return 0;
}

static int check_axis_constraint(double target, int id, const char* move_type, int axis_no)
{
    int in_range = 1;
    double nl = axes[axis_no].min_pos_limit;
    double pl = axes[axis_no].max_pos_limit;

    if (target < nl)
    {
        in_range = 0;
        reportError(AXIS_EXCEED_NEG_LIMIT, "move on line %d would exceed axis %d negative limit", id, axis_no);
    }

    if (target > pl)
    {
        in_range = 0;
        reportError(AXIS_EXCEED_POS_LIMIT, "move on line %d would exceed axis %d positive limit", id, axis_no);
    }

    return in_range;
}

/* inRange() returns non-zero if the position lies within the joint
 limits, or 0 if not.  It also reports an error for each joint limit
 violation.  It's possible to get more than one violation per move. */
static int inRange(ZucPose pos, int id, const char* move_type)
{
    return 1;
    // double joint_pos[ZUCMOT_MAX_JOINTS];
    // int joint_num;
    // zucmot_joint_t *joint;
    int in_range = 1;

    if (in_range && check_axis_constraint(pos.tran.x, id, move_type, 0) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.tran.y, id, move_type, 1) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.tran.z, id, move_type, 2) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.a, id, move_type, 3) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.b, id, move_type, 4) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.c, id, move_type, 5) == 0)
        in_range = 0;
    // if(check_axis_constraint(pos.u, id, move_type, 6) == 0)
    //     in_range = 0;
    // if(check_axis_constraint(pos.v, id, move_type, 7) == 0)
    //     in_range = 0;
    // if(check_axis_constraint(pos.w, id, move_type, 8) == 0)
    //     in_range = 0;

    /* Now, check that the endpoint puts the joints within their limits too */

    /* fill in all joints with 0 */

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++) {
    // 	joint_pos[joint_num] = 0;
    // }

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints /*zucmotConfig->numJoints*/; joint_num++) {
    // 	/* point to joint data */
    // 	joint = &joints[joint_num];

    // 	if (!GET_JOINT_ACTIVE_FLAG(joint)) {
    // 		/* if joint is not active, don't even look at its limits */
    // 		continue;
    // 	}

    // 	if (!isfinite(joint_pos[joint_num])) {
    // 		reportError(KINEMATICS_FAILED, ("%s move on line %d gave non-finite joint location on joint %d"), move_type, id, ONE_BASED(joint_num));
    // 		in_range = 0;
    // 		continue;
    // 	}
    // 	if (joint_pos[joint_num] > joint->max_pos_limit) {
    // 		in_range = 0;
    // 		reportError(JOINT_HIT_POS_SLIMIT_J1 + (joint_num<<16), ("%s move on line %d would exceed joint %d's positive limit"), move_type, id, ONE_BASED(joint_num));
    // 	}

    // 	if (joint_pos[joint_num] < joint->min_pos_limit) {
    // 		in_range = 0;
    // 		reportError(JOINT_HIT_NEG_SLIMIT_J1 + (joint_num<<16), ("%s move on line %d : %f would exceed joint %d's negative limit: %f"), move_type, id, joint_pos[joint_num], ONE_BASED(joint_num), joint->min_pos_limit);
    // 	}
    // }
    return in_range;
}

/* limits_ok() returns 1 if none of the hard limits are set,
 0 if any are set. Called on a linear and circular move. */
static int limits_ok(void)
{
    int joint_num;
    zucmot_joint_t* joint;

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        /* point to joint data */
        joint = &joints[joint_num];
        if (!GET_JOINT_ACTIVE_FLAG(joint))
        {
            /* if joint is not active, don't even look at its limits */
            continue;
        }

        if (GET_JOINT_PHL_FLAG(joint) || GET_JOINT_NHL_FLAG(joint))
        {
            return 0;
        }
        if ((joint->pos_cmd > joint->max_pos_limit && fabs(joint->pos_cmd - joint->max_pos_limit) > 1e-3) ||
            (joint->pos_cmd < joint->min_pos_limit && fabs(joint->pos_cmd - joint->min_pos_limit) > 1e-3))
        {
            return 0;
        }
    }

    return 1;
}

int CmdRigidTap::process()
{
    /* most of this is taken from ZUCMOT_SET_LINE */
    /* zucmotDebug coord_tp up a linear move */
    /* requires coordinated mode, enable off, not on limits */
    MOTCMD_INFO("-----start ZUCMOT_RIGID_TAP-------\n");
    MOTCMD_DEBUG("RIGID_TAP");
    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    {
        reportError(MISCELLANEOUS_ERROR, ("need to be enabled, in coord mode for rigid tap move"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return -1;
    }
    else if (!inRange(trajrigidtap_.pos, id_, "Rigid tap"))
    {
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return -1;
    }
    else if (!limits_ok())
    {
        reportError(MISCELLANEOUS_ERROR, ("can't do rigid tap move with limits exceeded"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return -1;
    }

    /* append it to the zucmotDebug coord_tp */
    //    tpManager_tpSetId(id_);
    //    int res_addtap = tpManager_tpAddRigidTap(trajrigidtap_.pos, trajrigidtap_.vel, trajrigidtap_.ini_maxvel, trajrigidtap_.acc, zucmotStatus->enables_new);
    //    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();
    // if (res_addtap < 0)
    // {
    //     reportError(MISCELLANEOUS_ERROR, ("can't add rigid tap move at line %d, error code %d"), id_, res_addtap);
    //     //        tpManager_tpabort();
    //     SET_MOTION_ERROR_FLAG(1);
    //     return -1;
    // }
    // else
    // {
    //     SET_MOTION_ERROR_FLAG(0);
    // }
    return 0;
}

int CmdJogMovc::process()
{
    MOTCMD_INFO("-----start ZUCMOT_JOG_MOVC_ABS-------\n");
    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    {
        reportError(ROBOT_NOT_ENABLED, ("need to be enabled, in coord mode for  movc"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    EndCond condition;
    condition.di_type = jog_.end_cond[0];
    condition.di_index = jog_.end_cond[1];
    condition.di_stat = jog_.end_cond[2];
    unsigned char enables = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    if (jog_.moveOpt)
        enables = 0;
    //    tpManager_tpSetId(jog_.executing_line_id);
    if (fabs(jog_.tol) > 1e-6)
    {
        //        tpManager_tpSetTermCond(TC_TERM_COND_PARABOLIC, fabs(jog_.tol));
    }
    else
    {
        //        tpManager_tpSetTermCond(TC_TERM_COND_STOP, 0.0);
    }
    // int res_addline =
    //        tpManager_tpAddMovc(jog_.cartPoseEx, jog_.cartPose, jog_.vel, jog_.vel, jog_.acc, 0, enables, 0, jog_.circle_cnt, condition, jog_.circle_mode);
    //    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();
    // if (res_addline < 0 || res_addline == TP_ERR_KINE)
    // {
    //     if (res_addline == TP_ERR_KINE)
    //     {
    //         reportError(FAILED_TO_ADD_MOVC, ("Failed to add  movc at line %d, singularity position or position out of reach."), jog_.executing_line_id);
    //     }
    //     else
    //     {
    //         reportError(FAILED_TO_ADD_MOVC, ("can't add movc at line %d, error code %d"), jog_.executing_line_id, res_addline);
    //     }
    //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
    //     //        tpManager_tpabort();
    //     SET_MOTION_ERROR_FLAG(1);
    //     return 0;
    // }
    // else
    // {
    //     SET_MOTION_ERROR_FLAG(0);
    //     rehomeAll = 1;
    // }
    //    //zucmotStatus->depth = tpManager_tpQueueDepth();

    return 0;
}

int CmdSetLine::process()
{
    /* WH printf
			MOTCMD_INFO("\n");
			MOTCMD_INFO("-----start ZUCMOT_SET_LINE-------\n");
			MOTCMD_INFO("-----end ZUCMOT_SET_LINE-------\n");
			MOTCMD_INFO("\n");
			*/
    MOTCMD_INFO("-----start ZUCMOT_SET_LINE-------\n");
    /* zucmotDebug coord_tp up a linear move */
    /* requires motion enabled, coordinated mode, not on limits */
    MOTCMD_DEBUG("SET_LINE");

    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    {
        reportError(ROBOT_NOT_ENABLED, ("need to be enabled, in coord mode for linear move"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    else if (!inRange(trajlinearmov_.pos, id_, "Linear"))
    {
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    else if (!limits_ok())
    {
        reportError(TARGET_POS_EXCEED_LIMIT, ("can't do linear move with limits exceeded"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }

    EndCond condition;
    /* append it to the zucmotDebug coord_tp */
    //    tpManager_tpSetId(id_);
    condition.di_type = trajlinearmov_.di_type;
    condition.di_index = trajlinearmov_.di_index;
    condition.di_stat = trajlinearmov_.di_state;
    char issue_atspeed = 0;

    //    int res_addline = tpManager_tpAddLine_toppra(trajlinearmov_.pos,
    //  trajlinearmov_.motion_type,
    //  trajlinearmov_.vel,
    //  trajlinearmov_.ini_maxvel,
    //  trajlinearmov_.acc,
    //  trajlinearmov_.jerk,
    //  trajlinearmov_.ori_vel,
    //  trajlinearmov_.ori_acc,
    //  zucmotStatus->enables_new,
    //  issue_atspeed,
    //  trajlinearmov_.turn,
    //  trajlinearmov_.using_abc_vel,
    //  condition,
    //  zucmotCommand->already_inpos,
    //  trajlinearmov_.planner_type,
    //  zucmotCommand->jpos_len,
    //  zucmotCommand->path_info,
    //  zucmotCommand->blend_path_info,
    //  zucmotCommand->cur_blend_left,
    //  zucmotCommand->cur_last_blend_len,
    //  zucmotCommand->blend_jpos_num + REFIT_NUM + 2,
    //  &zucmotCommand->cons_toppra,
    //  id_);

    //    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();
    // time_inst =  rtapi_get_time() - time_start;
    // time_max = time_inst > time_max ? time_inst : time_max;
    // if(time_cnt == 0) {
    //     time_ave = time_inst;
    // } else {
    //     time_ave = time_ave*(1.0*time_cnt/(time_cnt + 1)) + time_inst*1.0/(time_cnt + 1);
    // }
    // time_cnt++;

    // MOTCMD_INFO("tpAddLine time inst == %d\n\n", time_inst);
    // MOTCMD_INFO("tpAddLine time ave == %f\n\n", time_ave);
    // MOTCMD_INFO("tpAddLine time max == %d\n\n", time_max);

    // if (res_addline < 0 || res_addline == TP_ERR_KINE)
    // {
    //     if (res_addline == TP_ERR_KINE)
    //     {
    //         reportError(FAILED_TO_ADD_LINE, ("Failed to add linear move at line %d, singularity position or position out of reach./setline"), id_);
    //     }
    //     else
    //     {
    //         reportError(FAILED_TO_ADD_LINE, ("can't add linear move at line %d, error code %d"), id_, res_addline);
    //     }
    //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
    //     //        tpManager_tpabort();
    //     SET_MOTION_ERROR_FLAG(1);
    //     return 0;
    // }
    // else if (res_addline != 0)
    // {
    // }
    // else
    // {
    //     SET_MOTION_ERROR_FLAG(0);
    //     rehomeAll = 1;
    // }
    //    zucmotStatus->depth = tpManager_tpQueueDepth();
    // MOTCMD_INFO("[%ld] tpAddLine, target: %f, %f, %f\n", rtapi_get_clocks(), trajlinearmov_.pos.tran.x, trajlinearmov_.pos.tran.y, trajlinearmov_.pos.tran.z);
    // MOTCMD_INFO("[%ld] tpAddLine, after: zucmotStatus->depth = %d, zucmotStatus->id=%d\n", rtapi_get_clocks(), zucmotStatus->depth, zucmotStatus->id);
    return 0;
}

int CmdSetMovc::process()
{
    MOTCMD_INFO("-----start ZUCMOT_SET_MOVC-------\n");
    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    {
        reportError(ROBOT_NOT_ENABLED, ("need to be enabled, in coord mode for  movc"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    //    tpManager_tpSetId(id_);
    EndCond condition;
    condition.di_type = trajmovc_.di_type;
    condition.di_index = trajmovc_.di_index;
    condition.di_stat = trajmovc_.di_state;
    // MOTCMD_INFO("trajmovc_.circle_count = %f , %d\n",trajmovc_.circle_count, id_);

    //    int res_addline = tpManager_tpAddMovc_toppra(trajmovc_.movcMid,
    //  trajmovc_.movcEnd,
    //  trajmovc_.vel,
    //  trajmovc_.ini_maxvel,
    //  trajmovc_.acc,
    //  trajmovc_.jerk,
    //  zucmotStatus->enables_new,
    //  trajmovc_.using_abc_vel,
    //  trajmovc_.circle_count,
    //  trajmovc_.circlemode,
    //  condition,
    //  zucmotCommand->already_inpos,
    //  trajmovc_.planner_type,
    //  zucmotCommand->jpos_len,
    //  zucmotCommand->path_info,
    //  zucmotCommand->blend_path_info,
    //  zucmotCommand->cur_blend_left,
    //  zucmotCommand->cur_last_blend_len,
    //  zucmotCommand->blend_jpos_num + REFIT_NUM + 2,
    //  &zucmotCommand->cons_toppra,
    //  id_);
    //    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();

    // if (res_addline < 0 || res_addline == TP_ERR_KINE)
    // {
    //     if (res_addline == TP_ERR_KINE)
    //     {
    //         reportError(FAILED_TO_ADD_MOVC, ("Failed to add  movc at line %d, singularity position or position out of reach."), id_);
    //     }
    //     else
    //     {
    //         reportError(FAILED_TO_ADD_MOVC, ("can't add movc at line %d, error code %d"), id_, res_addline);
    //     }
    //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
    //     //        tpManager_tpabort();
    //     SET_MOTION_ERROR_FLAG(1);
    //     return 0;
    // }
    // else
    // {
    //     SET_MOTION_ERROR_FLAG(0);
    //     rehomeAll = 1;
    // }
    //    zucmotStatus->depth = tpManager_tpQueueDepth();
    return 0;
}

int CmdSetMovs::process()
{
    MOTCMD_INFO("-----start ZUCMOT_SET_MOVS-------\n");
    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    {
        reportError(ROBOT_NOT_ENABLED, ("need to be enabled, in coord mode for  movc"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
//    tpManager_tpSetId(id_);
//    tpManager_tpAddMovs(trajmovs_.a0, trajmovs_.a1, trajmovs_.a2, trajmovs_.a3, trajmovs_.time);
//    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();
#if 0
				int i;
				for(i = 0;i < 6;i ++)
				{
					MOTCMD_INFO(" [ZUCMOT_SET_MOVS]: ");
					MOTCMD_INFO(" a0[%d] = %f ",i,zucmotCommand->movs_a0[i]);
					if(5 == i)
					{
						MOTCMD_INFO("\n");	
					}
				}
				for(i = 0;i < 6;i ++)
				{
					MOTCMD_INFO(" [ZUCMOT_SET_MOVS]: ");
					MOTCMD_INFO(" a1[%d] = %f ",i,zucmotCommand->movs_a1[i]);
					if(5 == i)
					{
						MOTCMD_INFO("\n");	
					}
				}
				for(i = 0;i < 6;i ++)
				{
					MOTCMD_INFO(" [ZUCMOT_SET_MOVS] ");
					MOTCMD_INFO(" a2[%d] = %f ",i,zucmotCommand->movs_a2[i]);
					if(5 == i)
					{
						MOTCMD_INFO("\n");	
					}
				}
				for(i = 0;i < 6;i ++)
				{
					MOTCMD_INFO(" [ZUCMOT_SET_MOVS] ");
					MOTCMD_INFO(" a3[%d] = %f ",i,zucmotCommand->movs_a3[i]);
					if(5 == i)
					{
						MOTCMD_INFO("\n");	
					}
				}
				MOTCMD_INFO(" [ZUCMOT_SET_MOVS] ： time = %f \n",zucmotCommand->time);
#endif
    return 0;
}

int CmdSetCircle::process()
{
    /* zucmotDebu coord_tp up a circular move */
    /* requires coordinated mode, enable on, not on limits */
    MOTCMD_DEBUG("SET_CIRCLE");
    MOTCMD_INFO("-----start ZUCMOT_SET_CIRCLE-------\n");
    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    {
        reportError(ROBOT_NOT_ENABLED, ("need to be enabled, in coord mode for circular move"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    else if (!inRange(trajcircularmove_.pos, id_, "Circular"))
    {
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    else if (!limits_ok())
    {
        reportError(TARGET_POS_EXCEED_LIMIT, ("can't do circular move with limits exceeded"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }

    char issue_atspeed = 0;
    /* append it to the zucmotDebug coord_tp */
    //    tpManager_tpSetId(id_);
    //    int res_addcircle = tpManager_tpAddCircle(trajcircularmove_.pos,
    //   trajcircularmove_.center,
    //   trajcircularmove_.normal,
    //   trajcircularmove_.turn,
    //   trajcircularmove_.motion_type,
    //   trajcircularmove_.vel,
    //   trajcircularmove_.ini_maxvel,
    //   trajcircularmove_.acc,
    //   trajcircularmove_.jerk,
    //   zucmotStatus->enables_new,
    //   issue_atspeed);
    //    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();
    // if (res_addcircle < 0)
    // {
    //     reportError(FAILED_TO_ADD_LINE, ("can't add circular move at line %d, error code %d"), id_, res_addcircle);
    //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
    //     //        tpManager_tpabort();
    //     SET_MOTION_ERROR_FLAG(1);
    //     return 0;
    // }
    // else if (res_addcircle != 0)
    // {
    // }
    // else
    // {
    //     SET_MOTION_ERROR_FLAG(0);
    //     rehomeAll = 1;
    // }
    //    zucmotStatus->depth = tpManager_tpQueueDepth();
    return 0;
}

static int jointInRange(ZucPose pos)
{
    if ((pos.tran.x > joints[0].max_pos_limit) || (pos.tran.x < joints[0].min_pos_limit))
    {
        return 0;
    }
    if ((pos.tran.y > joints[1].max_pos_limit) || (pos.tran.y < joints[1].min_pos_limit))
    {
        return 0;
    }
    if ((pos.tran.z > joints[2].max_pos_limit) || (pos.tran.z < joints[2].min_pos_limit))
    {
        return 0;
    }
    if ((pos.a > joints[3].max_pos_limit) || (pos.a < joints[3].min_pos_limit))
    {
        return 0;
    }
    if ((pos.b > joints[4].max_pos_limit) || (pos.b < joints[4].min_pos_limit))
    {
        return 0;
    }
    if ((pos.c > joints[5].max_pos_limit) || (pos.c < joints[5].min_pos_limit))
    {
        return 0;
    }
    if (zucmotConfig->numJoints == 7)
    {
        if ((pos.u > joints[6].max_pos_limit) || (pos.u < joints[6].min_pos_limit))
        {
            return 0;
        }
    }
    return 1;
}

int CmdSetJointLine::process()
{
    /* zucmotDebug coord_tp up a linear move */
    /* requires motion enabled, coordinated mode, not on limits */
    MOTCMD_DEBUG("SET_JOINT_LINE");
    if (!GET_MOTION_ENABLE_FLAG())
    {
        reportError(ROBOT_NOT_ENABLED, ("need to be enabled, in coord mode for joint move"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }
    else if (!limits_ok())
    { /*to be fixed, by zxqi*/
        reportError(TARGET_POS_EXCEED_LIMIT, ("can't do joint move with limits exceeded"));
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }

    /* append it to the zucmotDebug coord_tp */
    //    tpManager_tpSetId(id_);
    EndCond condition;
    /*just hardcode the motion type, it's used to identify a joint movement*/
    condition.di_type = trajjointmov_.di_type;
    condition.di_index = trajjointmov_.di_index;
    condition.di_stat = trajjointmov_.di_state;

    if (!jointInRange(trajjointmov_.pos))
    {
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
        reportError(TARGET_POS_OUTOF_REACH, ("can't add joint move at line %d, position out of reach."), id_);
        //        tpManager_tpabort();
        SET_MOTION_ERROR_FLAG(1);
        return 0;
    }

    zucmotStatus->tpaddjointline_joint_vel_limit_zero = 0;
    char issue_atspeed = 0;
    //    int res_addjoint = tpManager_tpAddJointLine_toppra(trajjointmov_.pos,
    //    ZUC_MOTION_TYPE_JOINT_LINE,
    //    trajjointmov_.vel,
    //    trajjointmov_.ini_maxvel,
    //    trajjointmov_.acc,
    //    trajjointmov_.jerk,
    //    zucmotStatus->enables_new,
    //    issue_atspeed,
    //    trajjointmov_.turn,
    //    condition,
    //    zucmotCommand->joint,
    //    zucmotCommand->already_inpos,
    //    trajjointmov_.planner_type,
    //    zucmotCommand->jpos_len,
    //    zucmotCommand->path_info,
    //    zucmotCommand->blend_path_info,
    //    zucmotCommand->cur_blend_left,
    //    zucmotCommand->cur_last_blend_len,
    //    zucmotCommand->blend_jpos_num + REFIT_NUM + 2,
    //    &zucmotCommand->cons_toppra,
    //    id_);
    //    zucmotStatus->errAddLine = tpManager_tpGetErrAddLine();

    // MOTCMD_INFO(" ################ command: %f %f %f %f %f %f\n",
    //             zucmotCommand->path_info[0].jtor_a[0],
    //             zucmotCommand->path_info[0].jtor_a[1],
    //             zucmotCommand->path_info[0].jtor_a[2],
    //             zucmotCommand->path_info[0].jtor_a[3],
    //             zucmotCommand->path_info[0].jtor_a[4],
    //             zucmotCommand->path_info[0].jtor_a[5]);
    // if (res_addjoint < 0)
    // {
    //     //if (zucmotStatus->tpaddjointline_joint_vel_limit_zero != 1)
    //     reportError(FAILED_TO_ADD_JOINT_LINE, ("can't add joint move at line %d, error code %d!"), id_, res_addjoint);
    //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
    //     //        tpManager_tpabort();
    //     SET_MOTION_ERROR_FLAG(1);
    //     return 0;
    // }
    // else if (res_addjoint != 0)
    // {
    // }
    // else
    // {
    //     SET_MOTION_ERROR_FLAG(0);
    //     /* set flag that indicates all joints need rehoming, if any
    // 			 joint is moved in joint mode, for machines with no forward
    // 			 kins */
    //     rehomeAll = 1;
    // }
    //    zucmotStatus->depth = tpManager_tpQueueDepth();
    return 0;
}

int CmdSetRobotToolId::active_callback() { return 0; }

int CmdSetRobotToolId::process()
{
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->get_set_intf();
    set_intf->set_robot_tool(id_);
    return 0;
}

int CmdSetRobotUserId::process()
{
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->get_set_intf();
    set_intf->set_usr_frame(id_);
    return 0;
}

int CmdInCoordMode::active_callback()
{
    // if (status.protective_stop == 1)
    // {
    //     rcs_print("Cannot do movj when in protective_stop status\n");
    //     return -1;
    // }
    // if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG())
    // {
    //     reportError(OPERATION_IN_WRONG_MODE, ("need to be enabled, in coord mode for linear move#{\"fixed_key\":[\"NOCOORD\",\"JOG_MOVJ\"]}"));
    //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_COMMAND;
    //     SET_MOTION_ERROR_FLAG(1);
    //     return -1;
    // }
    // if (zucmotStatus->three_position_enable_limit == 1)
    // {
    //     //三位置开关处于打开状态不能运动
    //     reportError(ERROR_THREE_POSITION_ENABLING_LIMIT, "Can't jog when the three positions enable limits");
    //     return -1;
    // }

    return 0;
}

int CmdInCoordMode::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    if (robot->get_cur_plan_mode() != axisgroup::PFsmState::PlanMode::PLAN_COORD)
    {
        if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
        {
            MOTCMD_ERR("Robot %d is not ready for a coord command : moving state: %s, plan mode : %s",
                       group_id_,
                       axisgroup::AxisGroup::run_state_string(robot->get_state()),
                       axisgroup::Robot::get_plan_mode_stirng(robot->get_cur_plan_mode()));
            return -1;
        }
        // 自动切至free模式
        robot->switch_plan_mode(axisgroup::PFsmState::PlanMode::PLAN_COORD);
    }
    return 0;
}

int CmdJogMovj::active_callback()
{
    int err = CmdInCoordMode::active_callback();
    if (err)
    {
        return err;
    }
    return 0;
}

int CmdJogMovj::process()
{
    // #TODO LW 速度限制
    CmdInCoordMode::process();
    MOTCMD_INFO("-----Add ZUCMOT_JOG_MOVJ_ABS-------\n");
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto coord = std::static_pointer_cast<axisgroup::PFsmCoord>(robot->get_coord_pfsm());
    int joint_num = robot->joint_num();

    double tp_goal_joint_pos[ZUCMOT_MAX_AXIS] = {0};
    coord->tp_intf_->tpManager_tpGetGoalJointPos(tp_goal_joint_pos);

    RobJointVal absJointPos;
    RobJointVal deltJointPos;
    int jntId = 0;
    // calculate the incr and target position
    if (jog_.is_cpos == 0)  // 关节运动
    {
        for (jntId = 0; jntId < joint_num; jntId++)
        {
            if (jog_.moveOpt)  // 相对运动
            {
                absJointPos[jntId] = jog_.posCmd[jntId] + tp_goal_joint_pos[jntId];
                deltJointPos[jntId] = jog_.posCmd[jntId];
            }
            else  // 绝对运动
            {
                absJointPos[jntId] = jog_.posCmd[jntId];
                deltJointPos[jntId] = jog_.posCmd[jntId] - tp_goal_joint_pos[jntId];
            }
        }
        MOTCMD_INFO("JOG MOVJ joint pos: %s", absJointPos.val_str().c_str());
    }
    else
    {
        ZucPose tarPose = {};
        for (jntId = 0; jntId < joint_num; ++jntId) { absJointPos[jntId] = tp_goal_joint_pos[jntId]; }
        robot->kine()->fkine(RobJointVal(absJointPos),
                             tarPose,
                             &(axisgroup::RobotToolMgr::get_rob_tool(robot->tool_id_).pose()),
                             &(axisgroup::UsrFrameMgr::get_usr_frame(robot->wobj_id_).world(robot->base_frame_id_)));

        if (jog_.moveOpt)
        {
            tarPose.tran.x = jog_.posCmd[0] + tarPose.tran.x;
            tarPose.tran.y = jog_.posCmd[1] + tarPose.tran.y;
            tarPose.tran.z = jog_.posCmd[2] + tarPose.tran.z;
            tarPose.a = jog_.posCmd[3] + tarPose.a;
            tarPose.b = jog_.posCmd[4] + tarPose.b;
            tarPose.c = jog_.posCmd[5] + tarPose.c;
        }
        else
        {
            tarPose.tran.x = jog_.posCmd[0];
            tarPose.tran.y = jog_.posCmd[1];
            tarPose.tran.z = jog_.posCmd[2];
            tarPose.a = jog_.posCmd[3];
            tarPose.b = jog_.posCmd[4];
            tarPose.c = jog_.posCmd[5];
        }
        MOTCMD_INFO("JOG MOVJ cartesian pos: %lf, %lf, %lf, %lf, %lf, %lf", tarPose.tran.x, tarPose.tran.y, tarPose.tran.z, tarPose.a, tarPose.b, tarPose.c);

        // inverse kinematics --> get the target joint position
        kine::KineItf::kine_Conf kine_cfg;
        int kine_err = robot->kine()->ikine(tarPose,
                                            absJointPos,
                                            kine_cfg,
                                            &(axisgroup::RobotToolMgr::get_rob_tool(robot->tool_id_).pose()),
                                            &(axisgroup::UsrFrameMgr::get_usr_frame(robot->wobj_id_).world(robot->base_frame_id_)));
        if (kine_err != 0)
        {
            reportError(TARGET_POS_OUTOF_REACH, "can't add joint move, position out of reach.");
            return -1;
        };
        for (jntId = 0; jntId < joint_num; ++jntId) { deltJointPos[jntId] = absJointPos[jntId] - tp_goal_joint_pos[jntId]; }
    }

    auto config = robot->get_robot_active_config();
    for (size_t i = 0; i < robot->joint_num(); i++)
    {
        if (absJointPos[i] > config.joint_limit.max[i] || absJointPos[i] < config.joint_limit.min[i])
        {
            MOTCMD_ERR("JOG_MULTI, out of reach: axis_num %d target %lf", i, absJointPos[i]);
            reportError(TARGET_POS_EXCEED_LIMIT, "The target position exceeds the machine's limits.");
            return -1;
        }
    }

    // calculate the pivot joint for planning
    double maxTime = 0.0;
    double maxTimeVel = jog_.vel;
    int maxTimeJointNum = 0;
    for (jntId = 0; jntId < joint_num; jntId++)
    {
        double vThis = fabs(jog_.vel);
        double tThis = 0.0;
        if (fabs(jog_.vel) > config.joint_limit.vel[jntId])
        {
            vThis = joints[jntId].vel_limit;
        }
        tThis = fabs(deltJointPos[jntId] / vThis);
        if (maxTime < tThis)
        {
            maxTime = tThis;
            maxTimeJointNum = jntId;
            maxTimeVel = vThis;
        }
    }

    EndCond condition;
    ZucPose tarJointPos;
    tarJointPos.tran.x = absJointPos[0];
    tarJointPos.tran.y = absJointPos[1];
    tarJointPos.tran.z = absJointPos[2];
    tarJointPos.a = absJointPos[3];
    tarJointPos.b = absJointPos[4];
    tarJointPos.c = absJointPos[5];
    tarJointPos.u = absJointPos[6];
    condition.di_type = jog_.end_cond[0];
    condition.di_index = jog_.end_cond[1];
    condition.di_stat = jog_.end_cond[2];

    coord->tp_intf_->tpManager_tpSetId(jog_.executing_line_id);
    if (fabs(jog_.tol) > 1e-6)
    {
        coord->tp_intf_->tpManager_tpSetTermCond(TC_TERM_COND_PARABOLIC, fabs(jog_.tol));
    }
    else
    {
        coord->tp_intf_->tpManager_tpSetTermCond(TC_TERM_COND_STOP, 0.0);
    }

    unsigned char enables = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    if (jog_.moveOpt)
        enables = 0;

    int res = coord->tp_intf_->tpManager_tpAddJointLine_toppra(tarJointPos,
                                                               ZUC_MOTION_TYPE_JOINT_LINE,
                                                               maxTimeVel,
                                                               maxTimeVel,
                                                               jog_.acc,
                                                               0,
                                                               enables,
                                                               0,
                                                               -1,
                                                               condition,
                                                               maxTimeJointNum,
                                                               0,
                                                               0,
                                                               0,
                                                               zucmotCommand->path_info,
                                                               zucmotCommand->blend_path_info,
                                                               0,
                                                               0,
                                                               0,
                                                               &zucmotCommand->cons_toppra,
                                                               0);

    if (res)
    {
        MOTCMD_INFO("failed to add joint line: %d\n", res);
    }
    zucmotStatus->errAddLine = coord->tp_intf_->tpManager_tpGetErrAddLine();
    return 0;
}

int CmdJogMovl::active_callback()
{
    int err = CmdInCoordMode::active_callback();
    if (err)
    {
        return err;
    }
    return 0;
}

int CmdJogMovl::process()
{
    CmdInCoordMode::process();
    rtapi_print("-----Add ZUCMOT_JOG_MOVL-------\n");
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto coord = std::static_pointer_cast<axisgroup::PFsmCoord>(robot->get_coord_pfsm());
    int joint_num = robot->joint_num();
    auto config = robot->get_robot_active_config();

    // #TODO 应该使用JOG限制的速度吗？
    jog_.vel = jog_.vel < config.traj.vel.trans ? jog_.vel : config.traj.vel.trans;
    jog_.acc = jog_.acc < config.traj.acc.trans ? jog_.acc : config.traj.acc.trans;
    jog_.ori_vel = jog_.ori_vel < config.traj.vel.ori ? jog_.ori_vel : config.traj.vel.ori;
    jog_.ori_acc = jog_.ori_acc < config.traj.acc.ori ? jog_.ori_acc : config.traj.acc.ori;

    ZucPose goalPose;
    coord->tp_intf_->tpManager_tpGetGoalPose(&goalPose);

    ZucPose tarPose = {};
    double deltO, deltP, deltPos[3];
    PmRpy rpyTarget, rpyCurr;
    PmQuaternion quatTarget, quatCurr, quatDelt;

    if (type_ == JogMovlType::JOG_MOVL_INCR)
    {
        PmRpy oldOri, deltOri, newOri;

        tarPose.tran.x = jog_.cartPose.tran.x + goalPose.tran.x;
        tarPose.tran.y = jog_.cartPose.tran.y + goalPose.tran.y;
        tarPose.tran.z = jog_.cartPose.tran.z + goalPose.tran.z;
        oldOri.y = goalPose.a * PM_PI / 180.0;
        oldOri.p = goalPose.b * PM_PI / 180.0;
        oldOri.r = goalPose.c * PM_PI / 180.0;
        deltOri.y = jog_.cartPose.a * PM_PI / 180.0;
        deltOri.p = jog_.cartPose.b * PM_PI / 180.0;
        deltOri.r = jog_.cartPose.c * PM_PI / 180.0;
        pmRpyRpyAdd(&oldOri, &deltOri, &newOri);
        tarPose.a = newOri.y * 180.0 / PM_PI;
        tarPose.b = newOri.p * 180.0 / PM_PI;
        tarPose.c = newOri.r * 180.0 / PM_PI;
        deltPos[0] = jog_.cartPose.tran.x;
        deltPos[1] = jog_.cartPose.tran.y;
        deltPos[2] = jog_.cartPose.tran.z;
    }
    else if (type_ == JogMovlType::JOG_MOVL_TOOL_INCR)
    {
        getToolMovPos(&goalPose, &jog_.cartPose, &tarPose);
        deltPos[0] = tarPose.tran.x - goalPose.tran.x;
        deltPos[1] = tarPose.tran.y - goalPose.tran.y;
        deltPos[2] = tarPose.tran.z - goalPose.tran.z;
    }
    else
    {
        tarPose = jog_.cartPose;
        deltPos[0] = jog_.cartPose.tran.x - goalPose.tran.x;
        deltPos[1] = jog_.cartPose.tran.y - goalPose.tran.y;
        deltPos[2] = jog_.cartPose.tran.z - goalPose.tran.z;
    }

    if (!inRange(tarPose, -1, "Linear"))
    {
        MOTCMD_ERR("JogMovl: target position out of range\n");
        coord->tp_intf_->tpManager_tpabort();
        return -1;
    }
    else if (!limits_ok())
    {
        reportError(TARGET_POS_EXCEED_LIMIT, ("can't do linear move with limits exceeded"));
        MOTCMD_ERR("joint cmd pos:%f %f %f %f %f %f %f out of limits\n",
                   joints[0].pos_cmd,
                   joints[1].pos_cmd,
                   joints[2].pos_cmd,
                   joints[3].pos_cmd,
                   joints[4].pos_cmd,
                   joints[5].pos_cmd);
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        coord->tp_intf_->tpManager_tpabort();
        return -1;
    }
    EndCond condition;
    condition.di_type = jog_.end_cond[0];
    condition.di_index = jog_.end_cond[1];
    condition.di_stat = jog_.end_cond[2];

    unsigned char enables = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    if (jog_.moveOpt)
        enables = 0;

    rpyTarget.y = goalPose.a * PM_PI / 180.0;
    rpyTarget.p = goalPose.b * PM_PI / 180.0;
    rpyTarget.r = goalPose.c * PM_PI / 180.0;

    rpyCurr.y = tarPose.a * PM_PI / 180.0;
    rpyCurr.p = tarPose.b * PM_PI / 180.0;
    rpyCurr.r = tarPose.c * PM_PI / 180.0;

    pmRpyQuatConvert(&rpyTarget, &quatTarget);
    pmRpyQuatConvert(&rpyCurr, &quatCurr);
    pmQuatInv(&quatCurr, &quatCurr);
    pmQuatQuatMult(&quatTarget, &quatCurr, &quatDelt);
    deltO = fabs(2 * acos(quatDelt.s) * 180.0 / PM_PI);
    deltP = sqrt(deltPos[0] * deltPos[0] + deltPos[1] * deltPos[1] + deltPos[2] * deltPos[2]);

    double tl, ta;
    double vel, acc;
    int using_abc_vel;
    if (!enables)
    {
        ta = deltO ? fabs(deltO / 30.0) : 0.0;  // #FIXME 看起来不对
        tl = fabs(deltP / jog_.vel);
        // rtapi_print("enable == 1, ta = %f, tl = %f\n", ta, tl);
        if (tl < ta)
        {
            acc = MAX_JOG_AXIS_ACC_ROT;
            using_abc_vel = 1;
            vel = 30;
        }
        else
        {
            using_abc_vel = 0;
            acc = jog_.acc;  //MAX_JOG_AXIS_ACC_TRANS;
            vel = jog_.vel;
        }
    }
    else
    {
        // jog mode
        int i;
        double acc_a = config.traj.acc.ori;
        double vel_a = config.traj.vel.ori;
        double acc_l = jog_.acc;
        double vel_l = jog_.vel;
        //通过T型速度计算位置、姿态运动所需时间
        acc_l = jog_.acc;
        vel_l = jog_.vel;
        ta = coord->tp_intf_->tpManager_calTimeTTraj(deltO, vel_a, acc_a);
        tl = coord->tp_intf_->tpManager_calTimeTTraj(deltP, vel_l, acc_l);
        if (tl < ta)
        {
            using_abc_vel = 1;
            acc = acc_a;
            vel = vel_a;
        }
        else
        {
            using_abc_vel = 0;
            acc = jog_.acc;  //MAX_JOG_AXIS_ACC_TRANS;
            vel = jog_.vel;
        }
        // rtapi_print("enable == 0, ta = %f, tl = %f, zucmotCommand->vel = %f\n", ta, tl, zucmotCommand->vel);
    }
    coord->tp_intf_->tpManager_tpSetId(jog_.executing_line_id);
    double tol = fabs(jog_.tol);
    if (fabs(jog_.tol) > 1e-6)
    {
        coord->tp_intf_->tpManager_tpSetTermCond(TC_TERM_COND_PARABOLIC, tol);
    }
    else
    {
        coord->tp_intf_->tpManager_tpSetTermCond(TC_TERM_COND_STOP, 0.0);
    }
    MOTCMD_DEBUG("JOG MOVL Target: %lf, %lf, %lf, %lf, %lf, %lf", tarPose.tran.x, tarPose.tran.y, tarPose.tran.z, tarPose.a, tarPose.b, tarPose.c);
    int ret = coord->tp_intf_->tpManager_tpAddLine(
        tarPose, ZUC_MOTION_TYPE_TRAVERSE, vel, vel, acc, jog_.ori_vel, jog_.ori_acc, 0, enables, 0, -1, using_abc_vel, condition);

    // coord->tp_intf_->tpManager_tpGetErrAddLine();    // #TODO LW

    if (ret < 0 || ret == TP_ERR_KINE)
    {
        if (ret == TP_ERR_KINE)
        {
            reportError(FAILED_TO_ADD_LINE,
                        ("Failed to add linear move at line %d, singularity position or position out of reach./ZUCMOT_JOG_MOVL_ABS"),
                        zucmotCommand->id);
        }
        else
        {
            reportError(FAILED_TO_ADD_LINE, ("can't add linear move at line %d, error code %d"), zucmotCommand->id, ret);
        }
        coord->tp_intf_->tpManager_tpabort();
        return -1;
    }
    return 0;
}

int CmdPause::process()
{
    /* pause the motion */
    /* can happen at any time */
    MOTCMD_DEBUG("PAUSE");
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->pause();
    return 0;
}

int CmdResume::process()
{
    /* resume paused motion, can happen at any time */
    MOTCMD_DEBUG("RESUME");
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->resume();
    // zucmotDebug->stepping = 0;
    //    // tpManager_tpresume();
    // zucmotStatus->paused = 0;
    // MOTCMD_INFO("1: zucmotDebug->stepping set to 0, zucmotStatus->paused = %d\n", zucmotStatus->paused);
    return 0;
}

int CmdRapidScale::active_callback()
{
    if (scale_ < 0.0)
    {
        return -1;
    }
    if (zucmotStatus->percentage_mode_level == 0 && !zucmotStatus->safetyguard_lock) {}
    else
    {
        reportError(CANNOT_SET_RAPIDRATE_IN_SAFETYGUARD, "MOTION: please reset safety guard before setting override!");
        return -1;
    }
    return 0;
}

int CmdRapidScale::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    MOTCMD_DEBUG("RAPID SCALE change from %lf to %lf", robot->config_.rapid_scale, scale_);
    robot->config_.rapid_scale = scale_;  // 实时生效
    return 0;
}